#include "myInit.h"

/* FDCAN中断 ***************************************************************/

void HAL_FDCAN_RxFifo0Callback(FDCAN_HandleTypeDef *hfdcan, uint32_t RxFifo0ITs)
{
    if ((hfdcan->Instance == FDCAN1) || (hfdcan->Instance == FDCAN2) || (hfdcan->Instance == FDCAN3))
    {
		static user_FDCAN_Rx_Struct FDCAN_Rx_struct = {0};          //创建CAN接收结构体
		static uint8_t ID = 0;
	
		user_get_FDCAN_FIFO0_Msgs(hfdcan, &FDCAN_Rx_struct);    //接收CAN报文
		
		if (!gMotorRxInitFinishFlag)    //如果电机接收初始化没完成, 不处理接收到的数据
		{
			return;
		}
        ID = 0;
        ID += VESC_Analysis(&FDCAN_Rx_struct);
        ID += DJI_Analysis(&FDCAN_Rx_struct);
	#if ANGLE_SOURCE == ANGLE_SOURCE_CAN
		ID += AS5047P_CAN_Analysis(&FDCAN_Rx_struct);
	#endif
		
		
		switch (ID)
		{
			case NetLID:
				NetLRxFlag = 1;
				break;
			
			case NetRID:
				NetRRxFlag = 1;
				break;
			
			default:
				break;
		}
		
        if (ID != 0)
        {
            for (uint8_t i = 0; i < CANNum; i++)
            {
                if (ID == IDArray[i])
                {
                    IDFlagArray[i] = 1;
                    return;
                }
            }
        }
    }
}

/* 定时器中断 ***************************************************************/

void my_HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
    static uint16_t sCount500 = 0;

    static float sShootNowAngle_buff = 0.f;
    static float sShootDetlaAngle = 0.f;
    static float sShootSpeedLastAngle = 0.f;
	

    if(htim == &htim5)
    {
		{	//当检测到FDCAN总线因发送错误过多自动关闭时重启
			if (hfdcan1.Instance->PSR & FDCAN_PSR_BO)		
			{
				HAL_FDCAN_Stop(&hfdcan1);
				USER_FDCAN1_INIT(&hfdcan1);
			}
			if (hfdcan2.Instance->PSR & FDCAN_PSR_BO)
			{
				HAL_FDCAN_Stop(&hfdcan2);
				USER_FDCAN2_INIT(&hfdcan2);
			}
			if (hfdcan3.Instance->PSR & FDCAN_PSR_BO)
			{
				HAL_FDCAN_Stop(&hfdcan3);
				USER_FDCAN3_INIT(&hfdcan3);
			}
		}
		

        /* 500ms检查一次电机是否连接 */
        sCount500++;
        if (sCount500 >= 250)
        {
            sCount500 = 0;
            for (uint8_t i = 0; i < CANNum; i++)
            {
                if (IDFlagArray[i] == 0)        //电机通讯错误
                {
                    switch (IDArray[i])
                    {
                        case ShootID:
							gErrState.shootErr = true;
                            //推射电机通讯错误, 接收不到推射电机传回来的数据
                            break;
						
						default:
							break;

                    }
                    //MY_ERROR(); 错误处理的地方

                } 
                IDFlagArray[i] = 0;
            }
        }
        /*****************************/

        /* 推射编码器读角度 */

	#if ANGLE_SOURCE == ANGLE_SOURCE_AS5047P
		if (gMechInitFinishAndStartEncFlag)		//如果推射电机设置完原点
		{
			if (shootEnc.pf->read(&shootEnc))               //从推射VESC获得转发的角度
			{
				gShootReadAngle = shootEnc.angle; 

				sShootDetlaAngle = gShootReadAngle - gShootLastAngle;
				if (sShootDetlaAngle > 180)
				{
					sShootNowAngle_buff -= 360;
				}
				else if (sShootDetlaAngle < -180)
				{
					sShootNowAngle_buff += 360;
				}

				sShootNowAngle_buff += sShootDetlaAngle;

				gShootNowAngle = sShootNowAngle_buff;
				gShootLastAngle = gShootReadAngle;
			}
			else
			{
				
				static float ssShootFalseCount10 = 0;
				ssShootFalseCount10++;
				if (ssShootFalseCount10 >= 10)
				{
					gErrState.shootErr = true;
					//推射通讯错误 接收不到数据十次 20ms
				}
			}

			#if ENC_SPEED_ENABLE    //用编码器测推射电机的转速
				static uint8_t sCount100 = 0;
				sCount100++;
				if (sCount100 == 50)
				{
					sCount100 = 0;
					gShootSpeed = (gShootNowAngle - sShootSpeedLastAngle) * 10.f / 6.f;
					sShootSpeedLastAngle = gShootNowAngle;
				}
			#endif
			/********************/
		
		}
	#elif ANGLE_SOURCE == ANGLE_SOURCE_CAN
		gShootNowAngle_buff = AS5047P_CAN_GetPos();
		if (gShootNowAngle_buff)
		{
			gShootNowAngle = -gShootNowAngle_buff;
		}
	#endif
    }
}


/* FDCAN发送任务 ***************************************************************/
#define refreshCAN1()	{											\
	while(xQueueReceive(other_CAN1Handle,&sMotorToCAN,1) == pdTRUE)	\
	{																\
		sRxDataFlag = 1;											\
		switch(sMotorToCAN.ID)										\
		{															\
			case WheelLFID:											\
				sWheelData[0].Data = sMotorToCAN.Data;				\
				sWheelData[0].eMode = sMotorToCAN.eMode;			\
				break;												\
			case WheelRFID:											\
				sWheelData[1].Data = sMotorToCAN.Data;				\
				sWheelData[1].eMode = sMotorToCAN.eMode;			\
				break;												\
			case WheelLBID:											\
				sWheelData[2].Data = sMotorToCAN.Data;				\
				sWheelData[2].eMode = sMotorToCAN.eMode;			\
				break;												\
			case WheelRBID:											\
				sWheelData[3].Data = sMotorToCAN.Data;				\
				sWheelData[3].eMode = sMotorToCAN.eMode;			\
				break;												\
																	\
			case HelmLFID:											\
				sHelmData[0].Data = sMotorToCAN.Data;				\
				break;												\
			case HelmRFID:											\
				sHelmData[1].Data = sMotorToCAN.Data;				\
				break;												\
			case HelmLBID:											\
				sHelmData[2].Data = sMotorToCAN.Data;				\
				break;												\
			case HelmRBID:											\
				sHelmData[3].Data = sMotorToCAN.Data;				\
				break;												\
																	\
			default:												\
				break;												\
		}															\
	}																\
	if(sRxDataFlag) 												\
	{																\
		sRxDataFlag = 0;											\
		osDelay(1);													\
	}																\
}						

void can1Task(void *argument)
{
	static MotorToCAN sMotorToCAN;
	static MotorToCAN sWheelData[4] = {{.eMode = eMotorMode_Speed}, {.eMode = eMotorMode_Speed}, {.eMode = eMotorMode_Speed}, {.eMode = eMotorMode_Speed}};
	static MotorToCAN sHelmData[4];
	static uint8_t sRxDataFlag = 0;
	
    for(;;)
    {
		refreshCAN1();
		if (sWheelData[0].eMode == eMotorMode_Speed)
			WheelLF.pf->speedMode(&WheelLF, sWheelData[0].Data);
		else if (sWheelData[0].eMode == eMotorMode_Pos)
			WheelLF.pf->posMode(&WheelLF, sWheelData[0].Data);
		else if (sWheelData[0].eMode == eMotorMode_Duty)
			WheelLF.pf->dutyMode(&WheelLF, sWheelData[0].Data);
		else if (sWheelData[0].eMode == eMotorMode_Break)
			WheelLF.pf->dynamicBreakMode(&WheelLF, 30);
		else if (sWheelData[0].eMode == eMotorMode_Block)
			WheelLF.pf->staticBreakMode(&WheelLF, 15);
//		else if (sWheelData[0].eMode == eMotorMode_Current)
//			WheelLF.pf->currentMode(&WheelLF, sWheelData[0].Data);
		
		refreshCAN1();
		if (sWheelData[1].eMode == eMotorMode_Speed)
			WheelRF.pf->speedMode(&WheelRF, sWheelData[1].Data);
		else if (sWheelData[1].eMode == eMotorMode_Pos)
			WheelRF.pf->posMode(&WheelRF, sWheelData[1].Data);
		else if (sWheelData[1].eMode == eMotorMode_Duty)
			WheelRF.pf->dutyMode(&WheelRF, sWheelData[1].Data);
		else if (sWheelData[1].eMode == eMotorMode_Break)
			WheelRF.pf->dynamicBreakMode(&WheelRF, 30);
		else if (sWheelData[1].eMode == eMotorMode_Block)
			WheelRF.pf->staticBreakMode(&WheelRF, 15);
//		else if (sWheelData[1].eMode == eMotorMode_Current)
//			WheelLF.pf->currentMode(&WheelRF, sWheelData[1].Data);
		
		refreshCAN1();
		if (sWheelData[2].eMode == eMotorMode_Speed)
			WheelLB.pf->speedMode(&WheelLB, sWheelData[2].Data);
		else if (sWheelData[2].eMode == eMotorMode_Pos) 
			WheelLB.pf->posMode(&WheelLB, sWheelData[2].Data);
		else if (sWheelData[2].eMode == eMotorMode_Duty)
			WheelLB.pf->dutyMode(&WheelLB, sWheelData[2].Data);
		else if (sWheelData[2].eMode == eMotorMode_Break)
			WheelLB.pf->dynamicBreakMode(&WheelLB, 30);
		else if (sWheelData[2].eMode == eMotorMode_Block)
			WheelLB.pf->staticBreakMode(&WheelLB, 15);
//		else if (sWheelData[2].eMode == eMotorMode_Current)
//			WheelLF.pf->currentMode(&WheelLB, sWheelData[2].Data);
		
		refreshCAN1();
		if (sWheelData[3].eMode == eMotorMode_Speed)
			WheelRB.pf->speedMode(&WheelRB, sWheelData[3].Data);
		else if (sWheelData[3].eMode == eMotorMode_Pos) 
			WheelRB.pf->posMode(&WheelRB, sWheelData[3].Data);
		else if (sWheelData[3].eMode == eMotorMode_Duty)
			WheelRB.pf->dutyMode(&WheelRB, sWheelData[3].Data);
		else if (sWheelData[3].eMode == eMotorMode_Break)
			WheelRB.pf->dynamicBreakMode(&WheelRB, 30);
		else if (sWheelData[3].eMode == eMotorMode_Block)
			WheelRB.pf->staticBreakMode(&WheelRB, 15);
//		else if (sWheelData[3].eMode == eMotorMode_Current)
//			WheelLF.pf->currentMode(&WheelRB, sWheelData[3].Data);
		
		refreshCAN1();
		HelmLF.pf->posMode(&HelmLF, sHelmData[0].Data, 0, 0);
		refreshCAN1();
		HelmRF.pf->posMode(&HelmRF, sHelmData[1].Data, 0, 0);
		refreshCAN1();
		HelmLB.pf->posMode(&HelmLB, sHelmData[2].Data, 0, 0);
		refreshCAN1();
		HelmRB.pf->posMode(&HelmRB, sHelmData[3].Data, 0, 0);
    }
}


/* LED函数 ***************************************************************/
uint8_t Aim_LED;
#define	LED_COLOR_WHITE		0xFF, 0xFF, 0xFF
#define LED_COLOR_BLUE 		0x55, 0x00, 0x00
#define LED_COLOR_RED 		0x00, 0xFF, 0x00
#define LED_COLOR_GREEN 	0x00, 0x00, 0x55
#define LED_COLOR_PINK	 	0x44, 0xFF, 0x11
#define LED_COLOR_YELLOW 	0x00, 0x77, 0x44
#define LED_COLOR_BROWN 	0x00, 0x88, 0x11
void ledTask(void *argument)
{
    static uint16_t sTick = 0;
	
    while (1)
    {
    #if SHOOT_ENABLE
        if (xTaskGetTickCount() < sTick + 500)
        {
            if (gErrState.pcErr)
            {
                LEDStrip.pf->setColor(&LEDStrip, 1, 15, LED_COLOR_RED);    		//小电脑错误, 红灯
            }
        } 
        else if ((xTaskGetTickCount() >= (sTick + 500)) && (xTaskGetTickCount() < (sTick + 1000)))
        {
            if (gErrState.shootErr)
            {
                LEDStrip.pf->setColor(&LEDStrip, 1, 15, LED_COLOR_YELLOW);    	//推射错误, 黄灯
            }
        }  
    #endif
        else 
        {
            sTick = xTaskGetTickCount();
        }

        if (!gErrState.pcErr && !gErrState.shootErr)
        {
            LEDStrip.pf->setColor(&LEDStrip, 1, 15, LED_COLOR_BLUE);            // 没有错误, 蓝灯    
        }
		
        switch(Aim_LED)
        {
            case 0x0A:  // 蓝色
                        LEDStrip.pf->setColor(&LEDStrip, 16, 32, LED_COLOR_BLUE);
                        break;
            case 0x0B:  // 白色
                        LEDStrip.pf->setColor(&LEDStrip, 16, 32, LED_COLOR_WHITE);
                        break;
            case 0x0C:  // 绿色
                        LEDStrip.pf->setColor(&LEDStrip, 16, 32, LED_COLOR_GREEN);
                        break;
            case 0x0D:  // 红色
                        LEDStrip.pf->setColor(&LEDStrip, 16, 32, LED_COLOR_RED);
                        break;
            case 0x0E:  // 粉色
                        LEDStrip.pf->setColor(&LEDStrip, 16, 32, LED_COLOR_PINK);
                        break;
            
            default:    break;
        }
        osDelay(10);
    } 
}
